% *****************************************************************************
%
% Copyright (C) 2007 A. Ismael F. Vaz and L. N. Vicente.
%
% http://www.norg.uminho.pt/aivaz
% http://www.mat.uc.pt/~lnv
%
% This library is free software; you can redistribute it and/or
% modify it under the terms of the GNU Lesser General Public
% License as published by the Free Software Foundation; either
% version 2.1 of the License, or (at your option) any later version.
%
% This library is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with this library; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%  The original code consists of multiple files.
%  This file simply collects the relevant files of PSwarm optimization program 
%     into a single file for convenience. 
% 
%  This file and minor changes in the code were made by KE.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file PSwarm.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [BestParticle, BestParticleObj, RunData]=PSwarm1(Problem, ...
    InitialPopulation, Options, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Particle swarm pattern search algorithm for global optimization.
%
% PSwarm algorithm takes advantage of the particle swarm algorithm in
% looking for a global optimum in the search phase of the pattern search
% algorithm. The pattern search enforces convergence for a local optimum.
%
%
% The pswarm MATLAB function syntax is
% [BestParticle, BestParticleObj, RunData] = PSwarm(Problem,
%                                  InitialPopulation, Options, P1, P2, ...)
%
% Input arguments:
%
%   Problem is a structure with some problem definitions
%
%     Problem.Variables        - Number of continuum real variables. If not
%                                specified then the size of the LB vector
%                                will be assigned to it.
%            .ObjFunction      - The name of the function to be called to
%                                when a objective function value is
%                                requested. This function syntax is
%                                [f]=myobj(x), where is a output argument
%                                - the objective function value - computed
%                                at (x), being x a vector with real
%                                variables.
%
%            .LB and .UB       - are column vector with the lower and upper
%                                bounds on the variables.
%
%            .A and .b         - Linear constraints Ax<=b
%
%   InitialPopulation is an array of structures.
%
%     InitialPopulation(1).x  -  Real variables initial guess to include in
%                                the initial population.
%                      (2).x  -  Second approximation for real ...
%       If the size of the structure array exceeds the population size then
%       the latter one will automatically be incremented.
%
%   Option is a structure with options to be passed to the algorithm. Call
%       pswarm('defaults') to obtain a list of the default options.
%
%   Just type opt=PSwarm('defaults') to obtain the default options for the
%       algorithm
%
%   P1, P2, ... are extra parameters passed to the objective evaluation
%       function
%
%
%
%
% Output arguments:
%
%   BestParticle              - The best particle obtained for the
%                               population (Leader)
%   BestParticleObj           - The best particle objective function
%                               obtained for the population (Leader
%                               objective function value)
%   RunData                   - Some statistics about the algorithm run
%
%
% Copyright (C) 2007 A. Ismael F. Vaz and L. N. Vicente.
%
% http://www.norg.uminho.pt/aivaz
% http://www.mat.uc.pt/~lnv
%
% This library is free software; you can redistribute it and/or
% modify it under the terms of the GNU Lesser General Public
% License as published by the Free Software Foundation; either
% version 2.1 of the License, or (at your option) any later version.
%
% This library is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with this library; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 04/04/2007


% Default options
%  Cognitial - cognitial parameter in the velocity equation
%  InerciaFinalWeight - The value of the inercia parameter in the velocity
%             at last iteration.
%  InerciaInitialWeight - The value of the inercia parameter in the
%             velocity equation at first iteration.
%  MaxObj - Maximum number of objective function evaluations. Since the
%             algorithm is population based this maximum number of
%             objective function evaluation may be sligtly exceeded.
%  MaxIter - Maximum number of iterations allowed.
%  Size - Population size.
%  Social - Social parameter in the velocity equation.
%  MaxVelocityFactor - Velocity will be projected in the set
%             (UB-LB)*MaxVelocityFactor.
%  CPTolerance - Stopping criteria tolerance.
%  InitialDelta - Initial pattern search grid step.
%  DeltaIncreseFactor - Delta will be increased by this factor (on
%             successful poll steps).
%  DeltaDecreaseFactor - Delta willbe decreased by this factor (on
%             unsuccessful poll steps).
%  InitialDeltaFactor - The inicial Delta will be the min(UB-LB) times this
%             factor.
%  DegTolerance - Degenerancy tolerance.
%  LinearStepSize - Projection type on search step (particle swarm). =0 no
%               Step Size used on velocity . =1 Maximum Step Size if
%               computed. >1 Maximum Step Size if computed and velocity is
%               truncated for linear constraints.
%  EpsilonActive - Precision used to compute linear epsilon active
%               constraints.
%  TangentCone - 0 SID-PSM like version. 1 NOMAD like version. 2 SID-PSM
%               adapted (active constraints changes). 3 NullSpace version.
%               4 Simple QR factorization.
%
%  Trials - number of trials in generating initial guess.
%  IPrint - Iteration Print. If <0 no print is done. If =0 print the best
%           point found. If =n, n>0, print the best objective function
%           value each n iterations.
%  OutputFCN - Function to be called each IPrint iterations.
%  Vectorized - Call objective function in a vectorized way (=1) or point
%           by point (=0). In the vectorized version the poll step is
%           non-oportunistic, while in the not vectorized version the poll
%           step is oportunistic.
%  SearchType - search step type. 0 - no search. 1 - particle swarm. 2 -
%           RBF. 3 - Quadratic Model with Minumum Frobenius Norm.
%  Cache - 1 - cache objective function values
%  LoadCache - 1 - Load cache from file at startup
%  SaveCache - 1 - Save cache to file at end
%  CacheFile - Cache filename
%  RBFAlgo - Algorithm to be used to minimize the objective function, 1 -
%           DCA, 2 - fmincon
%  RBFPoint - 0 - Only point in the poll step to build the RBF model, 1 -
%           all cached points
%  PollSort - 0 - no sort in the poll step, 1 - sort by model
%  TRType - Trust region type - 0 - based on the grid parameter, 1 - based on ratio
%

  % The DefaultOpt controls also the available options
  % An option not provided in the DefaultOpt will be an invalid option
  DefaultOpt = struct('Cognitial', 0.5, 'InerciaFinalWeight', 0.4,...
      'InerciaInitialWeight', 0.9, ...
      'MaxObj', 2000, 'MaxIter', 2000, 'Size', 20, 'Social', 0.5, ...
      'MaxVelocityFactor', 0.5, 'CPTolerance', 1.0e-5, 'InitialDelta', 20.0, ...
      'DeltaIncreaseFactor', 2.0, 'DeltaDecreaseFactor', 0.5, ...
      'InitialDeltaFactor', 5.0, 'IPrint', 10, 'DegTolerance', 0.001, ...
      'LinearStepSize', 1, 'EpsilonActive', 1e-1, 'TangentCone', 4,...
      'Trials', 1000, 'MVETrials', 10, 'OutputFCN', 'outputfcn', 'Vectorized', 0,...
      'SearchType', 1, 'Cache', 0, 'CacheChunks', 100, 'LoadCache', 0,...
      'SaveCache', 0, 'CacheFile', 'PSCache', 'RBFAlgo', 1, 'MFNAlgo', 1,...
      'DCARhoFactor', 5*10^-3,'RBFPoints',1,'PollSort',0,'TRType',0);

  % With no arguments just print an error
  if nargin==0 && nargout==0
      error('PSwarm:Arguments', 'Invalid number of arguments. Type ''help PSwarm'' to obtain help.');
  end


  % If just 'defaults' passed in, return the default options in X
  if nargin==1 && nargout <= 1 && isequal(Problem,'defaults')
     BestParticle = DefaultOpt;
     return
  end


  % Check parameters consistence
  if nargin < 1
    error('pswarm:AtLeastOneInput','PSwarm requests at least one imput (Problem definition).');
  end

  % If parameters are missing just define them as empty
  if nargin < 3, Options=[];
      if nargin < 2, InitialPopulation = [];
      end
  end

  % Problem should be a structure
  if ~isstruct(Problem)
      error('pswarm:StructProblem', 'First parameter must be a struct.');
  end


  % Do some sanity checkup on the user provided parameters and data
  % We need an objective function
  if ~isfield(Problem,'ObjFunction') || isempty(Problem.ObjFunction)
      error('pswarm:ObjMissing', 'Objective function name is missing.');
  end

  % and simple bound for particle swarm
  if ~isfield(Problem,'LB') || ~isfield(Problem,'UB') || ~isnumeric(Problem.LB) || ~isnumeric(Problem.UB) || ...
          isempty(Problem.LB) || isempty(Problem.UB)
      error('pswarm:Bounds', 'PSwarm relies on bound constraints on all variables.');
  end

  % Bound arrays must have the same size
  if length(Problem.LB)~=length(Problem.UB)
      error('pswarm:BoundsSize', 'Lower bound and upper bound arrays length mismatch.');
  end

  % Compute the number of variables
  % If the user provides these number check for correctness, otherwise try to
  % compute them from the size of the bound constraints
  if (~isfield(Problem, 'Variables') || isempty(Problem.Variables))
      % default is to consider all variables as real
      Problem.Variables=length(Problem.LB);
  end

  % Check for computed number of variables
  if Problem.Variables<0 || Problem.Variables>length(Problem.LB)
      error('pswarm:VariablesNumber', 'Number of variables do not agree with bound constraints');
  end

  % Be sure to have a column vector
  ULBSize=size(Problem.LB);
  if(ULBSize(1)~=Problem.Variables)
      Problem.LB=Problem.LB';
  end
  ULBSize=size(Problem.UB);
  if(ULBSize(1)~=Problem.Variables)
      Problem.UB=Problem.UB';
  end
  for i=1:length(InitialPopulation)
      % Be sure to have a column vector
      XSize=size(InitialPopulation(i).x);
      if (XSize(1)~=Problem.Variables && XSize(2)~=Problem.Variables)
          error('pswarm:InitialPopulation', 'Initial Population size mismatch problem size');
      end
      if(XSize(1)~=Problem.Variables)
          InitialPopulation(i).x=InitialPopulation(i).x';
      end
  end

  % Do some checkup in the linear constraints if they exist
  if isfield(Problem, 'A')
      if isempty(Problem.A)
          Problem.mLinear=0;
      else
          [Problem.mLinear, nVars]=size(Problem.A);
          if Problem.Variables ~= nVars;
              error('pswarm:Linear', 'Number of variables for Linear constraints do not agree.');
          end
      end

      % Check for independent term
      if ~isfield(Problem, 'b') || Problem.mLinear ~= length(Problem.b)
          error('pswarm:IndependentTerm', 'Size of Linear independent term does not agree or is not defined.');
      end
  else
      % Always define linear constraints
      Problem.mLinear=0;
      Problem.A=[];
      Problem.b=[];
  end




  % check for invalid options (just to warn user about type errors).
  if(isstruct(Options))
      InvalidOptions=setdiff(fieldnames(Options),fieldnames(DefaultOpt));
      if(~isempty(InvalidOptions))
          for i=1:length(InvalidOptions)
              fprintf('Invalid option(s) --> ');
              fprintf('%s\n',char(InvalidOptions(i)));
          end
          error('pswarm:InvalidOption', 'Invalid option(s) (see above)');
      end
  end

  % Initialize options. GetOption returns the user specified value, if the
  % option exists. Otherwise returns the default option.

  Problem.MaxIterations=GetOption('MaxIter',Options,DefaultOpt);
  Problem.MaxEvals=GetOption('MaxObj',Options,DefaultOpt);
  Problem.IPrint=GetOption('IPrint', Options, DefaultOpt);
  Problem.OutputFCN=GetOption('OutputFCN', Options, DefaultOpt);
  Problem.IncreaseDelta=GetOption('DeltaIncreaseFactor',Options,DefaultOpt);
  Problem.DecreaseDelta=GetOption('DeltaDecreaseFactor',Options,DefaultOpt);
  Problem.DegTolerance=GetOption('DegTolerance',Options,DefaultOpt);
  Problem.Tolerance=GetOption('CPTolerance', Options, DefaultOpt);
  Problem.LinearStepSize=GetOption('LinearStepSize', Options, DefaultOpt);
  Problem.EpsilonActive=GetOption('EpsilonActive', Options, DefaultOpt);
  Problem.TangentCone=GetOption('TangentCone', Options, DefaultOpt);
  Problem.Trials=GetOption('Trials', Options, DefaultOpt);
  Problem.MVETrials=GetOption('MVETrials', Options, DefaultOpt);
  Problem.Vectorized=GetOption('Vectorized', Options, DefaultOpt);
  Problem.SearchType=GetOption('SearchType', Options, DefaultOpt);
  %Problem.InitialDelta=GetOption('InitialDelta', Options, DefaultOpt);
  Problem.InitialDeltaFactor=GetOption('InitialDeltaFactor', Options, DefaultOpt);
  Problem.Cache=GetOption('Cache', Options, DefaultOpt);
  if(Problem.Cache)
      Problem.CacheChunks=GetOption('CacheChunks', Options, DefaultOpt);
      Problem.LoadCache=GetOption('LoadCache', Options, DefaultOpt);
      Problem.SaveCache=GetOption('SaveCache', Options, DefaultOpt);
      Problem.CacheFile=GetOption('CacheFile', Options, DefaultOpt);
  end

  Population.Size=GetOption('Size', Options, DefaultOpt);
  Problem.PollSort=GetOption('PollSort', Options, DefaultOpt);
  Problem.TRType=GetOption('TRType', Options, DefaultOpt);


  % Initialize statistics counters

  % Number of objective function calls. This is the number of calls to
  % Problem.ObjFunction. Since particle swarm relies in a infinite penalty
  % function strategy the penalty number of evaluation may be different.
  Problem.Stats.ObjFunCounter=0;

  % Number of penalty function calls. This is the number of calls to
  % the barrier penalty function.
  Problem.Stats.PenaltyFunCounter=0;

  % Number of penalty function calls. This is the number of calls to
  % the barrier penalty function.
  Problem.Stats.RealObjFunCounter=0;

  % Number of Poll steps taken
  Problem.Stats.PollSteps=0;

  % How many poll step had success
  Problem.Stats.SuccPollSteps=0;

  % How many times Degeneracy happens in poll step
  Problem.Stats.Degenerate=0;


  % Initialize patter search. Initialize the coordinate search directions.
  [Problem, Population]=InitPatternSearch(Problem, Population, Options, DefaultOpt);


  switch Problem.SearchType
      case 0 % Empty search step

      case 1 % Particle swarm search step
          Problem.InerciaInitial=GetOption('InerciaInitialWeight',Options,DefaultOpt);
          Problem.InerciaFinal=GetOption('InerciaFinalWeight',Options,DefaultOpt);
          Problem.Cognitial=GetOption('Cognitial',Options,DefaultOpt);
          Problem.Social=GetOption('Social',Options,DefaultOpt);
          Problem.MaxVelocityVect=(Problem.UB(1:Problem.Variables)-Problem.LB(1:Problem.Variables))*...
              GetOption('MaxVelocityFactor',Options,DefaultOpt);

          if(Problem.Cache && Problem.IPrint>0)
              fprintf('Cache of objective function values is enabled with');
              fprintf(' the particle swarm search step\n');
              fprintf('Performance may be reduced due to memory usage\n');
          end
      case 2 % RBF search step
          if(~Problem.Cache)
              fprintf('Cache of the objective function values must be enabled');
              fprintf(' for the RBF search step.\n Enabling it...');
              Problem.Cache=1;
          end

          % For DC algorithm
          Problem.Stats.RBF.nModels=0;
          Problem.Stats.RBF.nModelsPreviousSuccess=0;
          Problem.Stats.RBF.Success=0;
          Problem.RBFAlgo=GetOption('RBFAlgo', Options, DefaultOpt);
          Problem.RBFPoints=GetOption('RBFPoints', Options, DefaultOpt);
          Problem.DCARhoFactor=GetOption('DCARhoFactor', Options, DefaultOpt);

          % Trust Region parameters - for now not allowed to be changed by
          % user
          if(Problem.TRType==1)
              Problem.RBFEta0=Problem.Tolerance^0.5;
              Problem.RBFEta1=0.2;
              Problem.RBFGamma0=0.5;
              Problem.RBFGamma1=2;
              Problem.RBFDeltaMax=Problem.InitialDelta;
              if(Problem.RBFDeltaMax>1)
                  Population.RBFDelta=Problem.RBFDeltaMax^0.5;
              else
                  Population.RBFDelta=Problem.RBFDeltaMax^2;
              end
          end

          % Model data handler (appdata)
          if(Problem.PollSort==1)
              Problem.RBFDataHandle=0;

              % Reset data
              RBFData=[];
              setappdata(Problem.RBFDataHandle,'RBFData',RBFData);
          end

      case 3 % MFN search step
          if(~Problem.Cache)
              fprintf('Cache of the objective function values must be enabled');
              fprintf(' for the MFN search step.\n Enabling it...');
              Problem.Cache=1;
          end

          % For DC algorithm
          Problem.Stats.MFN.nModels=0;
          Problem.Stats.MFN.nModelsPreviousSuccess=0;
          Problem.Stats.MFN.Success=0;
          Problem.MFNAlgo=GetOption('MFNAlgo', Options, DefaultOpt);
          Problem.DCARhoFactor=GetOption('DCARhoFactor', Options, DefaultOpt);

          % Trust Region parameters - for now not allowed to be changed by
          % user
          if(Problem.TRType==1)
              Problem.RBFEta0=Problem.Tolerance^0.5;
              Problem.RBFEta1=0.2;
              Problem.RBFGamma0=0.5;
              Problem.RBFGamma1=2;
              Problem.RBFDeltaMax=Problem.InitialDelta;
              if(Problem.RBFDeltaMax>1)
                  Population.MFNDelta=Problem.RBFDeltaMax^0.5;
              else
                  Population.MFNDelta=Problem.RBFDeltaMax^2;
              end
          end

          % Model data handler (appdata)
          if(Problem.PollSort==1)
              Problem.MFNDataHandle=0;

              % Reset data
              MFNData=[];
              setappdata(Problem.MFNDataHandle,'MFNData',MFNData);
          end

      otherwise
          error('Unknown search step type')
  end



  % Initialize counters
  % Iteration counter
  Problem.Stats.IterCounter=0;
  % How many consecutive iterations without success (progress in the leader
  % particle).
  IterUnsuccess=0;

  % User was already warned of unfeasible leader?
  Problem.FeasWarning=0;

  % Cache initialization, before InitPopulation
  Problem.CacheData=InitCache(Problem);

  % Generate initial population. Include initial population provided by user
  %  if available
  [Problem,Population]=InitPopulation(Problem, Population, InitialPopulation, ...
      GetOption('Size', Options, DefaultOpt), varargin{:});

  % Main cycle of the algorithm
  % Run pattern search until no success is attained. Call for a poll step in
  % the leader particle whenever no success is recorded.

  % Start clock
  tic;

  % Stop if the maximum number of iterations or objective function
  % evaluations is reached.
  while(Problem.Stats.IterCounter<Problem.MaxIterations && Problem.Stats.ObjFunCounter<Problem.MaxEvals)

      if Problem.IPrint>0 && ...
          (Problem.Stats.IterCounter==1 || mod(Problem.Stats.IterCounter,Problem.IPrint)==0)
              res=feval(Problem.OutputFCN, Problem, Population);
              if(isnumeric(res) && res<0)
                  if Problem.IPrint>=0
                      disp('Stopping due to user request');
                  end
                  break;
              end
      end

      if(CheckStoppingCriteria(Problem, Population))
          break;
      end

      % Increment iteration counter.    
      Problem.Stats.IterCounter=Problem.Stats.IterCounter+1;

      switch Problem.SearchType
          case 0 % Empty search step
              % no sucess to force a poll step
              Success=0;
          case 1 % Particle swarm search step
              [Success,Problem,Population]=ParticleSwarm(Problem,Population, varargin{:});
          case 2 % RBF search step
              [Success,Problem,Population]=RBF(Problem, Population, varargin{:});
          case 3 % MFN search step
              [Success,Problem,Population]=Quad_MFN(Problem, Population, varargin{:});
          otherwise
              error('Unknown search step type')
      end

      % Successful iteration?
      if ~Success
          % No success in search phase. Proceed to a poll step if possible.
          if Population.Delta >= Problem.Tolerance
              [Problem,Population]=PollStep(Problem,Population, ...
                  varargin{:});
              Problem.Stats.PollSteps=Problem.Stats.PollSteps+1;
              % Reset on the number of unsuccessful iterations without a poll
              % step
              IterUnsuccess=0;
          else
              % An unsuccessful iteration without poll step
              IterUnsuccess=IterUnsuccess+1;
          end
      else
          % Success
          IterUnsuccess=0;
          % Leader changed.
          % Increase Delta. Reset on the local search.
          if Population.Delta<Problem.InitialDelta
              Population.Delta=Population.Delta*Problem.IncreaseDelta;
          end
          % check for lower bounds
          if Population.Delta<Problem.Tolerance
              Population.Delta=2*Problem.Tolerance;
          end
      end


      [Problem, Population]=UpdateInfo(Problem, Population);

  end


  % End of main cycle ...


  % print final time
  if Problem.IPrint>=0
      toc;
  end

  % Save cache file
  if(Problem.Cache && Problem.SaveCache)
      CacheData=Problem.CacheData;
      save(Problem.CacheFile,'CacheData');
  end

  % Print if it was stopped due to the maximum of iterations or objective
  % function evaluations
  if Problem.Stats.IterCounter>=Problem.MaxIterations || Problem.Stats.ObjFunCounter>=Problem.MaxEvals
      if Problem.IPrint>=0
          disp('Maximum number of iterations or objective function evaluations reached');
      end
  end

  % Print for feasibility of the leader
  if Problem.IPrint>=0 && Problem.mLinear>0
      if max(Problem.A*Population.y(Population.Leader,:)'-Problem.b)<=eps
          fprintf('Best particle is linear feasible\n');
      else
          error('Best particle is NOT linear feasible');
      end
  end

  % return leader position and objective function value
  BestParticle=[Population.y(Population.Leader,:)]';
  BestParticleObj=Population.fy(Population.Leader);
  RunData=Problem.Stats;

  if(isfield(Problem, 'Cache') && Problem.Cache>0)
      RunData.Cache.hits=Problem.CacheData.hits;
      RunData.Cache.size=Problem.CacheData.Counter;
  end


  if Problem.IPrint>=0
      % display some statistics
      disp(Problem.Stats);
  end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  subrotine GetOption
%
%  Input:
%    Option - option to get the value
%    Options - a list of options provided by user
%    DefaultOpt -  a list of default options
%
%  Output:
%    Value - The value specified by user for Option or the default
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Value]=GetOption(Option, Options, DefaultOpt)

  % Check for user provided options
  %if isempty(Options) || ~isstruct(Options)
      % User does not provides Options
  %    Value=DefaultOpt.(Option);
  %    return;
  %end

  % Try the option provided by user
  try
      Value=Options.(Option);
  catch
      Value=[];
  end

  % Option not provided by user
  if isempty(Value)
      Value=DefaultOpt.(Option);    
  end

end

























%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file CheckStoppingCriteria.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [stop]=CheckStoppingCriteria(Problem, Population)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subrotine CheckStoppingCriteria
%
%  This subroutine check for the stopping criteria
%
%
%  Input:
%    Problem - The problem structure
%    Population - The population structure
%
%  Output:
%    1 to stop
%    0 don't stop
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 03-06-2009


  switch Problem.SearchType
      case 0 % Empty search step
          % Stop if pattern search Delta is bellow Tolerance
          if Population.Delta<Problem.Tolerance
              if Problem.IPrint>=0
                  disp('Stopping due to tolerance');
              end
              stop=1;
              return;
          end
      case 1 % Particle swarm search step
          % Stop also if particle swarm velocity is bellow Tolerance and pattern
          % search Delta is bellow Tolerance
          if Population.MaxVelocity<Problem.Tolerance && Population.Delta<Problem.Tolerance
              if Problem.IPrint>=0
                  disp('Stopping due to velocity and tolerance');
              end
              stop=1;
              return;
          end

          % Stop if the number of active particles is equal to 1 and pattern
          % search Tolerance was attained.
          if Population.ActiveParticles <=1 && Population.Delta<Problem.Tolerance
              if Problem.IPrint>=0
                  disp('Stopping due to single particle and tolerance');
              end
              stop=1;
              return;
          end
      case {2,3} % RBF or MFN search step
          % Stop if pattern search Delta is bellow Tolerance
          if Population.Delta<Problem.Tolerance
              if Problem.IPrint>=0
                  disp('Stopping due to tolerance');
              end
              stop=1;
              return;
          end
      otherwise
          error('Unknown search step type')
  end


  stop=0;
end




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file InitCache.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function CacheData=InitCache(Problem)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subrotine InitCache
%
%  This subroutine initializes the cache structure
%
%
%  Input:
%    Problem - The problem structure
%
%  Output:
%    CacheData - Cache data structure
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  if(~isfield(Problem, 'LoadCache'))
      CacheData=[];
      return;
  end

  % Assume load cache is not correct
  correct=0;
  % A Cache file exists and will be used
  if Problem.LoadCache && (exist(Problem.CacheFile,'file') || exist([char(Problem.CacheFile),'.mat'],'file'))
      if(Problem.IPrint>0)
          fprintf('Loading cache from file %s\n', char(Problem.CacheFile));
      end
      load(Problem.CacheFile,'CacheData');

      % check for cache correctness
      try
          correct=1;
          if ~isfield(CacheData,'xnorm')
              correct=0;
          end
          if ~isfield(CacheData,'step')
              correct=0;
          end
          if ~isfield(CacheData,'Counter')
              correct=0;
          end
          if ~isfield(CacheData,'Point')
              correct=0;
          else
              if ~isfield(CacheData.Point,'x')
                  correct=0;
              end
              if ~isfield(CacheData.Point,'fx')
                  correct=0;
              end
          end
      catch
          correct=0;
      end
  end

  % number of hits
  CacheData.hits=0;
  % Tolerance for compare
  CacheData.Tolerance=Problem.Tolerance/10;

  if(~correct)
      % number of points in the cache
      CacheData.Counter=0;
      % allocate memory in chunks
      Idx=1:Problem.CacheChunks;
      [CacheData.Point(Idx).x]=deal(zeros(Problem.Variables,1));
      [CacheData.Point(Idx).fx]=deal(0);
      [CacheData.xnorm(Idx)]=deal(0);
      [CacheData.step(Idx)]=deal(0);
  end
end




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file InitPatternSearch.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Problem, Population]=InitPatternSearch(Problem, Population, Options, DefaultOpt)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subrotine InitPatternSearch
%
%  This subroutine initializes the search directions for the poll step
%
%  User provided directions can be included in the
%       Problem.Poll.SearchDirections.UserSpecified matrix
%  The user provided search directions will be included prior to any other
%  directions.
%
%  Provides the coordinate directions
%
%  Input:
%    Problem - The problem structure
%    Options - user defined options
%    DefaultOpt - default options
%
%  Output:
%    Problem - The problem structure updated
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 01/12/2006
% updated             03/06/2009

  % Compute minimum delta for pattern search
  % The minimum delta available should be at least twice the Tolerance
  I=intersect(find(Problem.UB<Inf),find(Problem.LB>-Inf));
  if ~isempty(I)
      InitialDelta=min(Problem.UB(I)-Problem.LB(I));
      InitialDelta = InitialDelta / Problem.InitialDeltaFactor;
  else
      try
          InitialDelta = Options.InitialDelta;
      catch
          InitialDelta = DefaultOpt.InitialDelta;
      end
  end

  % A too small initial delta?
  if InitialDelta < sqrt(2*Problem.Tolerance)
      InitialDelta = sqrt(sqrt(Problem.Tolerance));
  end

  % Set initial delta
  Problem.InitialDelta=InitialDelta;


  % compute variables scaling
  FiniteVars=find(isfinite(Problem.LB.*Problem.UB));
  FiniteConst=find(isfinite(Problem.b));

  Problem.Scale=1;
  if ~isempty(FiniteVars)
      Problem.Scale = max([max(abs(Problem.UB(FiniteVars)-Problem.LB(FiniteVars))), Problem.Scale]);
  end
  if ~isempty(FiniteConst)
      Problem.Scale = max([max(abs(Problem.b(FiniteConst))), Problem.Scale]);
  end


  if Problem.IPrint>=0
      disp('Initial delta for pattern search ');
      disp(Problem.InitialDelta);
  end

  % Delta for the pattern search.
  Population.Delta=Problem.InitialDelta;


  % Keep track of what direction had success in the last poll step. The Delta
  % parameter is incremented only of a success occours twice in the same
  % direction.
  Problem.Poll.LastSuccess=[];


  % Coordinate Search for Variables
  % May not be used, but it is never changed in the algorithm
  Problem.Poll.SearchDirections.Base=[ones(Problem.Variables,1)...
                                     -ones(Problem.Variables,1)...
                                     eye(Problem.Variables) ...
                                     -eye(Problem.Variables)];

  % Linear search directions
  % Is computed during the algorithm. It's pointless to provide one.
  Problem.Poll.SearchDirections.Linear=[];

  % A set of directions specified by user
  % The computed search directions are appended to this one. If you know a
  % better directions order then you should provide it here.
  Problem.Poll.SearchDirections.UserSpecified =[];

end










%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file InitPopulation.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Problem,Population]=InitPopulation(Problem, Population, ...
    InitialPopulation, PopSize, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subrotine InitPopulation
%
%  This subroutine initializes population
%
%
%  Input:
%    Problem - The problem structure
%    InitPopulation - Initial guess population
%    PopSize - Population size
%    varargin - additional arguments for the objective function
%
%  Output:
%    Problem - The problem structure updated
%    Population - The population structure updated
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


  switch Problem.SearchType
      case {0,2,3} % Empty and RBF search step
          if(PopSize>1)
              fprintf('With a null/RBF search step the population size must be one\n');
              fprintf('Forcing to be one...\n');
              PopSize=1;
          end
          JustOne=1; % Only one initial guess
          if(length(InitialPopulation)>1)
              fprintf('With a null/RBF search step just one initial guess can be considered\n');
              fprintf('Using the first feasible one provided...\n');
          end
          % Just use the same code as particle swarm
          [Problem, Population]=InitPopSwarm(Problem, Population,...
              InitialPopulation, PopSize, JustOne, varargin);
          [Problem,Population.fy(Population.Leader)]=...
              PenaltyEval(Problem, Population.y(Population.Leader,:), 1, varargin{:});
      case 1 % Particle swarm search step
          JustOne=0; % Consider all the initial guesses provided
          [Problem, Population]=InitPopSwarm(Problem, Population,...
              InitialPopulation, PopSize, JustOne, varargin);

      otherwise
          error('Unknown search step type')
  end

end



function [Problem, Population]=InitPopSwarm(Problem, Population, ...
    InitialPopulation, Size, JustOne, varargin)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% subrotine InitPopulation
%    Randomly initialize the population
%    Include initial guesses, if provided by the user
%
% Input:
%   Problem - problem data
%   InitialPopulation - Inicial population provided by user
%   Size - Requested size for the population
%
% Output:
%   Problem - problem data (problem data may be update)
%   Population - population data
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 03-06-2009

  % Maximum velocity among all particles velocity
  Population.MaxVelocity=+Inf;

  % Leader is the first one
  Population.Leader=1;

  % Number of particles in initial population that were accepted
  nP=0;

  if Problem.IPrint>=0
      disp('Generating initial population');
  end

  % Check if user provides a valid initial population
  if ~isempty(InitialPopulation) && ~isstruct(InitialPopulation)
      error('pswarm:InitPopulation:InitialPopulation', 'Initial population must be defined in a structure.');
  else
      % Copy the initial population for the population and initialize them
      % This is only done if the particle, after projection, is linear
      % feasible
      for i=1:length(InitialPopulation)
          % Particle position.
          Population.x(nP+1,:)=Projection(InitialPopulation(i).x,...
                  Problem.LB(1:Problem.Variables), ...
                  Problem.UB(1:Problem.Variables));
          if Problem.mLinear==0 || max(Problem.A*Population.x(nP+1,:)'-Problem.b)<=eps
              %particle is linear feasible
              nP=nP+1;

              % Best particle position.
              Population.y(nP,:)=Population.x(nP,:);
              % Particle velocities.
              Population.vx(nP,:)=zeros(1,Problem.Variables);
              % Particle is active at begining
              Population.Active(nP)=true;
              Population.fy(i)=+Inf;
              %            [Problem,Population.fy(nP)]=+Inf;
              %                PenaltyEval(Problem, Population.x(nP,:), varargin{:});

              % if we request just one initial guess then return
              if(JustOne)
                  Population.ActiveParticles=1;
                  Population.Leader=1;
                  Population.Size=1;
                  return;
              end
          end
      end
  end

  if (nP<length(InitialPopulation))
      warning('Only %d particles were linear feasible after projection', nP);
  end

  % Check for size
  if nP>Size
      % User provided an initial feasible population greater than the population
      % size
      if Problem.IPrint>=0
          fprintf('Initial feasible population is greater than population size.\n');
          fprintf('Increasing population size from ');
          fprintf('%i',Size);
          fprintf(' to ');
          fprintf('%i',nP);
      end
      % Population size is increased to fit the number of initial guesses
      Population.Size=nP;
      Population.ActiveParticles=Population.Size;
      % no need to proceed
      return;
  else
      % Otherwise just accept the proposed population size
      Population.Size=Size;
  end

  % Population size can no longer increase
  Population.ActiveParticles=Population.Size;

  % No need to proceed if we have all the initial guesses 
  if(nP>=Population.Size)
      return;
  end

  % Ramdomly generate the remaining population
  % They must be linear feasible

  Ellipsoid=0;

  if Problem.mLinear > 0
      if Problem.IPrint>=0
          disp('Generating Ellipsoid');
      end

      Ellipsoid=1; % we are computing an ellipsoid

      [A,b]=RealBounds(Problem);

      % We have linear constraints, so take care of the polytope
      maxiter = 80; tol1 = 1.e-8; tol2 = 1.e-6;

      if (nP<=0) || max(Problem.A*Population.x(1,:)'-Problem.b)>=eps ...
              || max(Population.x(1,:)'-Problem.LB)>=eps ...
              || max(Problem.UB-Population.x(1,:)')>=eps
          % We do not have a linear feasible approximation
          % Get one from presolve
          [msg,x0] = mve_presolve(A, b, maxiter,...
              zeros(Problem.Variables,1), tol1, 0); % initial guess with zeros
          if msg(1) ~= 's'
              % error on presolver.
              if Problem.IPrint>=0
                  disp('MVE presolver error');
                  disp(msg);
                  disp('Trying to recover by adding fictitious bounds');
              end
              [A,b]=FicticiousBounds(Problem,A,b);
              [msg,x0] = mve_presolve(A, b, maxiter,...
                  zeros(Problem.Variables,1), tol1, 1);
              if msg(1) ~= 's'
                  if Problem.IPrint>=0
                      disp('MVE presolver error');
                      disp(msg);
                  end

                  trial=0;
                  [lb,ub]=GetBounds(Problem); % to randomly generate initial guess into bound constraints
                  while msg(1)~='s' && trial < Problem.MVETrials
                      [msg,x0] = mve_presolve(A, b, maxiter, unifrnd(lb,ub),...
                          tol1, 0);
                      if msg(1) ~= 's' && Problem.IPrint>=0
                          disp('MVE presolver error');
                          disp(msg);
                      end
                      trial=trial+1;
                  end
                  if msg(1) ~= 's'
                      if Problem.IPrint>=0
                          disp('MVE presolver error');
                          disp(msg);
                          disp('We are leaving the ellipsoid strategy');
                      end
                      Ellipsoid=0;
                  end
              end
          end
      else
          x0=Population.x(1,:)'; % First as initial guess
      end

      if(Ellipsoid==1)
          % Get the ellipsoid
          % use again real bounds
          [A,b]=RealBounds(Problem);

          [x,E2,msg] = mve_solver(A, b, x0, maxiter, tol2);

          if ~msg
              if Problem.IPrint>=0
                  disp('MVE solver error');
                  disp('Trying to recover by adding fictitious bounds');
              end

              [A,b]=FicticiousBounds(Problem,A,b);

              [x,E2,msg] = mve_solver(A, b, x0, maxiter, tol2);

              %if ~msg
              %    disp('MVE solver error');
              %    disp('We are leaving the ellipsoid strategy');
              %    Ellipsoid=0;
              %end
          end

          %if(msg)
              [E,out] = chol(E2);

              if(out>0)
                  if Problem.IPrint>=0
                      disp('Unable obtain ellipsoid');
                  end
                  Ellipsoid=0;
              else
                  E = E';
              end

              % x - elipsoid center
              % E - Elipsoid matrix

              %hold off;

              %draw_ellipse([Problem.A; eye(Problem.Variables);...
              %    -eye(Problem.Variables)],...
              %    [Problem.b; Problem.UB; -Problem.LB],x0,x,E,[]);
          %else
          %    disp('We are leaving the ellipsoid strategy');
          %    Ellipsoid=0;
          %end
      end
  end % End of ellipsoid computation

  if ~Ellipsoid % get real bounds
      [lb,ub]=GetBounds(Problem);
  end

  for i=nP+1:Population.Size
      % Particle positions.
      if Ellipsoid==0
          if Problem.mLinear>0
              Population.x(i,:)=GetFeasiblePoint(Problem,lb,ub);
          else
              Population.x(i,:)=unifrnd(lb,ub);
          end
      else
          tmppt=unifrnd(-ones(Problem.Variables,1), ones(Problem.Variables,1));
          tmppt=tmppt/sqrt(tmppt'*tmppt);
          Population.x(i,:) = Projection(x +...
              (unifrnd(0, 1))^(1/Problem.Variables)*E*tmppt,...
              Problem.LB(1:Problem.Variables), ...
              Problem.UB(1:Problem.Variables));
          if max(Problem.A*Population.x(i,:)'-Problem.b)>0
              error('Not feasible pop. Internal error. This should not happen.');
          end
      end
      % Best particle position.
      Population.y(i,:)=Population.x(i,:);
      % Particle velocities
      Population.vx(i,:)=zeros(1,Problem.Variables);
      % Particle active or inactive
      Population.Active(i)=true;
      Population.fy(i)=+Inf;
  %    [Problem,Population.fy(i)]=...
  %        PenaltyEval(Problem, Population.x(i,:), varargin{:});
  end


  % plot population
  %PlotPopulation(Problem, Population);

  % plot ellipsoid
  %t=0:0.1:2*pi;
  %ellipsoid=E*[cos(t);sin(t)];
  %plot(x(1)+ellipsoid(1,:),x(2)+ellipsoid(2,:));

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Plot the population
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function []=PlotPopulation(Problem, Population)

  if Problem.Variables ~= 2
      return;
  end

  hold on;

  for j=1:Population.Size
      if Population.Active(j)
          plot(Population.y(j,1),Population.y(j,2),'ok');
      end
  end
    
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Compute real bounds
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [A,b]=RealBounds(Problem)

  if Problem.mLinear>0
      % Construct the polytope with linear constraints
      Id=eye(Problem.Variables); % Identity matrix for bound constraints
      A=[Problem.A; Id(find(Problem.UB<Inf),:); -Id(find(Problem.LB>-Inf),:)];
      b=[Problem.b; Problem.UB(find(Problem.UB<Inf)); -Problem.LB(find(Problem.LB>-Inf))];
  else
      % Construct the polytope without linear constraints
      Id=eye(Problem.Variables); % Identity matrix for bound constraints
      A=[Id(find(Problem.UB<Inf),:); -Id(find(Problem.LB>-Inf),:)];
      b=[Problem.UB(find(Problem.UB<Inf)); -Problem.LB(find(Problem.LB>-Inf))];
  end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Compute ficticious bounds
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [A,b]=FicticiousBounds(Problem,RealA,Realb)

  A=RealA;
  b=Realb;
  Id=eye(Problem.Variables);

  I=intersect(find(Problem.UB<Inf),find(Problem.LB<=-Inf));
  % Upper bound finite and Lower bound not finite
  if ~isempty(I)
      A=[A; -Id(I,:)];
      % Fictitious bound is three times the distance to origin
      o_I=ones(length(I),1);
      b=[b; -min(-100*o_I,Problem.UB(I)-3*abs(Problem.UB(I)))];
  end
  I=intersect(find(Problem.LB>-Inf), find(Problem.UB>=Inf));
  % Lower bound finite and Upper bound not finite
  if ~isempty(I)
      A=[A; Id(I,:)];
      o_I=ones(length(I),1);
      % Fictitious bound is three times the distance to origin
      b=[b; max(100*o_I,Problem.LB(I)+3*abs(Problem.LB(I)))];
  end
  I=intersect(find(Problem.UB>=Inf),find(Problem.LB<=-Inf)); % Both limites not finite
  if ~isempty(I)
      A=[A; -Id(I,:); Id(I,:)];
      o_I=ones(length(I),1);
      b=[b; -o_I*min(-100, -10*min(Problem.LB>-Inf)); o_I*max(100, 10*max(Problem.UB<Inf))];
  end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Compute bounds (add ficticious if necessary)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [lb,ub]=GetBounds(Problem)

  lb=Problem.LB;
  ub=Problem.UB;

  I=intersect(find(Problem.UB<Inf),find(Problem.LB<=-Inf));
  % Upper bound finite and Lower bound not finite
  if ~isempty(I)
      o_I=ones(length(I),1);
      lb(I)=min(-100*o_I,-Problem.UB(I)+3*abs(Problem.UB(I)));
  end
  I=intersect(find(Problem.LB>-Inf), find(Problem.UB>=Inf));
  % Lower bound finite and Upper bound not finite
  if ~isempty(I)
      o_I=ones(length(I),1);
      ub(I)=max(100*o_I,Problem.LB(I)+3*abs(Problem.LB(I)));
  end
  I=intersect(find(Problem.UB>=Inf),find(Problem.LB<=-Inf)); % Both limites not finite
  if ~isempty(I)
      o_I=ones(length(I),1);
      lb(I)=o_I*min(-100, 10*min(Problem.LB>-Inf));
      ub(I)=o_I*max(100, 10*max(Problem.UB<Inf));
  end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Get Feasible point by try and error
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x]=GetFeasiblePoint(Problem,lb,ub)

  for i=1:Problem.Trials
      x=unifrnd(lb,ub);
      if max(Problem.A*x-Problem.b)<=0
          % Is linear feasible
          return;
      end
  end

  disp('Failed to generate feasible initial guess');
  error('Unable to proceed!');

end




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file mve_presolve.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [msg,x,t,s,y] = mve_presolve(A,b,maxiter, x0, tol, second)
% Solve LP: max t, s.t. Ax + t*e <= b

%--------------------------------------
% Yin Zhang, Rice University, 07/29/02
%--------------------------------------

  [m, n] = size(A);  bnrm = norm(b); 
  o_m = zeros(m,1);  o_n = zeros(n,1);
  e_m =  ones(m,1);  e_n =  ones(n,1);

  % initialize
  %x = o_n; y = e_m/m;
  x = x0; y = e_m/m; % aivaz
  t = min(b) - 1; s = b - t;

  dx = o_n; dxc = dx;
  ds = o_m; dsc = ds;
  dy = o_m; dyc = dy;
  dt = 0;   dtc = 0;
  tau0 = 0.995; 
  sigma0 = 0.2;
  msg = 'successful';

  p = 1:n;
  if issparse(A)
      absA = abs(A); 
      p = symmmd(absA'*absA);
  end
  p(n+1) = n+1;

  %fprintf('\n  Residuals:   Primal     Dual     Duality    Obj\n');
  %fprintf('  --------------------------------------------------\n');

  for iter = 0:maxiter

    % KKT residuals
    r1 = b - (A*x + s + t);
    r2 = -A'*y;
    r3 = 1 - sum(y);
    r4 = -s.*y;
    gap = -sum(r4);

    % relative residual norms and gap
    prif = norm(r1)/(1 + bnrm); 
    drif = norm([r2;r3]); 
    rgap = abs(b'*y - t)/(1 + abs(t)); 
    total_err = max([prif drif rgap]);

    % progress output & check stopping
    if (total_err < tol) 
   %     fprintf('  iter. %3i: %9.1e %9.1e %9.1e %9.1e\n',...
   %     iter,prif,drif,rgap,t); break; 
      break;
    end
   % fprintf('  iter. %3i: %9.1e %9.1e %9.1e %9.1e\n',...
   %            iter,prif,drif,rgap,t); 
    if dt > 1.e+3*bnrm | t > 1.e+6*bnrm
        msg = 'unbounded?'; break; 
    end  

    % Shur complement matrix
    d = min(5.e+15,y./s); 
    AtD = A'*spdiags(d,0,m,m);
    AtDe = AtD*e_m;  
    B = [AtD*A AtDe; AtDe' sum(d)];
    B = B + 1.e-14*speye(n+1);

    % Cholesky decomposition
    if ~issparse(A)
        [R,out] = chol(B);
    else
        [R,out] = cholinc(B(p,p),'inf');
    end

    if(out>0 && second==0)
        msg = 'Cholesky_factorization_error'; break;
    end

    % predictor step & length
    if(out==0)
        [dx,ds,dt,dy] = calcstep(A,R,p,s,y,r1,r2,r3,r4);
    else
        [dx,ds,dt,dy] = calcstepB(A,B,p,s,y,r1,r2,r3,r4);
    end


    alphap = -1/min([-1; ds./s]);
    alphad = -1/min([-1; dy./y]);

    % determine mu
    ratio = (s+alphap*ds)'*(y+alphad*dy)/gap;
    sigma = min(sigma0, ratio^2);
    mu = sigma*gap/m;

    % corrector and combined step & length
    if(out==0)
        [dxc,dsc,dtc,dyc] = calcstep(A,R,p,s,y,o_m,o_n,0,mu-ds.*dy);
    else
        [dxc,dsc,dtc,dyc] = calcstepB(A,B,p,s,y,o_m,o_n,0,mu-ds.*dy);
   end

    dx = dx + dxc; ds = ds + dsc; 
    dt = dt + dtc; dy = dy + dyc;
    alphap = -1/min([-.5; ds./s]);
    alphad = -1/min([-.5; dy./y]);

    % update iterates
    tau = max(tau0, 1-gap/m);
    alphap = min(1,tau*alphap);
    alphad = min(1,tau*alphad);
    x = x + alphap * dx;
    s = s + alphap * ds;
    t = t + alphap * dt; 
    y = y + alphad * dy;
  end
  if t < eps msg = 'no volume'; end
  %disp(msg);
end

  
%%%%%%%%%%%%%%%%%%%%%%%%%%
function [dx,ds,dt,dy] = calcstep(A,R,p,s,y,r1,r2,r3,r4);
  dxdt = zeros(size(R,1),1);
  tmp = (r1.*y-r4)./s;
  rhs = [r2+A'*tmp; r3+sum(tmp)]; 
  if ~issparse(A)
     dxdt = R\(R'\rhs);
  else
     dxdt(p) = R\(R'\rhs(p));
  end
  dx = dxdt(1:end-1); 
  dt = dxdt(end);
  ds = r1 - A*dx - dt;
  dy = (r4 - y.*ds)./s;
end
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%
function [dx,ds,dt,dy] = calcstepB(A,B,p,s,y,r1,r2,r3,r4);
  dxdt = zeros(size(B,1),1);
  tmp = (r1.*y-r4)./s;
  rhs = [r2+A'*tmp; r3+sum(tmp)]; 
  if ~issparse(A)
     dxdt = B\rhs;
  else
     dxdt(p) = B\rhs(p);
  end
  dx = dxdt(1:end-1); 
  dt = dxdt(end);
  ds = r1 - A*dx - dt;
  dy = (r4 - y.*ds)./s;
end


 
 
 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file mve_solver.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x,E2,msg,y,z,iter] = mve_solver(A,b,x0,maxiter,tol)
%  Find the maximum volume ellipsoid
%    {v:  v = x + Es, ||s|| <= 1}
%  inscribing a full-dimensional polytope
%          {v:  Av <= b}
%  Input:  A, b --- defining the polytope
%          x0 --- interior point (Ax0 < b)
%  Output: x --- center of the ellipsoid
%          E2 --- E'*E

%--------------------------------------
% Yin Zhang, Rice University, 07/29/02
%--------------------------------------

  t0 = cputime; 
  [m, n] = size(A);
  bnrm = norm(b); 

  if ~exist('maxiter') maxiter = 50; end;
  if ~exist('tol') tol = 1.e-4; end;
  minmu = 1.e-8; tau0 = .75;

  bmAx0 = b - A*x0;
  if any(bmAx0<=0) || any(isnan(x0))
  %    disp('Error: x0 not interior or NAN');
      x=x0;
      E2=[0 1;1 0]; % to make an error on the cholesky factorization
      msg=0;
      return;
  end

  A = sparse(1:m,1:m,1./bmAx0)*A; b = ones(m,1); 
  x = zeros(n,1); y = ones(m,1); bmAx = b;

  %fprintf('\n  Residuals:   Primal     Dual    Duality  logdet(E)\n');
  %fprintf('  --------------------------------------------------\n');

  res = 1; msg = 0;
  for iter=1:maxiter %----- loop starts -----

  if iter > 1
      bmAx = bmAx - astep*Adx;
  end

  Y = sparse(1:m,1:m,y);
  E2 = inv(full(A'*Y*A));
  Q = A*E2*A';
  h = sqrt(diag(Q));

  if iter==1
     t = min(bmAx./h); 
     y = y/t^2; h = t*h;
     z = max(1.e-1, bmAx-h);
     Q = t^2*Q; Y = Y/t^2;
  end

  yz = y.*z; yh = y.*h;
  gap = sum(yz)/m;
  rmu = min(.5, gap)*gap;
  rmu = max(rmu, minmu);

  R1 = -A'*yh;
  R2 = bmAx - h - z;
  R3 = rmu - yz;

  r1 = norm(R1,'inf');
  r2 = norm(R2,'inf');
  r3 = norm(R3,'inf');
  res = max([r1 r2 r3]);
  %objval = log(det(E2))/2;

  %fprintf('  iter %3i  ', iter);
  %fprintf('%9.1e %9.1e %9.1e  %9.3e\n', r2,r1,r3,objval);
  if res < tol*(1+bnrm) & rmu <= minmu 
  %   fprintf('  Converged!\n'); 
     x = x + x0; msg=1; break; 
  end

  YQ = Y*Q; YQQY = YQ.*YQ'; y2h = 2*yh; YA = Y*A;
  G  = YQQY + sparse(1:m,1:m,max(1.e-12,y2h.*z));
  T = G \ (sparse(1:m,1:m,h+z)*YA);
  ATP = (sparse(1:m,1:m,y2h)*T-YA)';

  R3Dy = R3./y; R23 = R2 - R3Dy;
  dx = (ATP*A)\(R1 + ATP*R23); Adx = A*dx;
  dyDy = G\(y2h.*(Adx - R23));
  dy = y.*dyDy;
  dz = R3Dy - z.*dyDy;

  ax = -1/min([-Adx./bmAx; -.5]);
  ay = -1/min([ dyDy; -.5]);
  az = -1/min([dz./z; -.5]); 
  tau = max(tau0, 1 - res);
  astep = tau*min([1 ax ay az]);

  x = x + astep*dx;
  y = y + astep*dy;
  z = z + astep*dz;

  end

  if(msg==0)
      x = x + x0;
  end

  fprintf('  CPU time: %g seconds in %d iterations\n', cputime-t0,iter);

end
 
 
 
 
 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file outputfcn.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [res]=outputfcn(Problem, Population)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  User function called each Option.IPrint iterations
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  if ~isstruct(Problem) || ~isstruct(Population)
      return;
  end

  if Problem.Stats.IterCounter<=0 % first call then print header
      fprintf('\n  Iter   Act     Leader     Objective    Delta');
      fprintf('\n  ----------------------------------------------\n');
  end

  fprintf('    %4i   %3i   %4i   %4.6e %4.6e\n', Problem.Stats.IterCounter, ...
      Population.ActiveParticles, Population.Leader,...
      Population.fy(Population.Leader), Population.Delta);

  % return a negative value to stop PSwarm execution
  res=1.0;

end









%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file ParticleSwarm.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Success,Problem,Population]=ParticleSwarm(Problem, Population, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subrotine InitSwarm
%
%  This subroutine implements the particle swarm search step
%
%
%  Input:
%    Problem - The problem structure
%    Population- The population structure
%
%  Output:
%    Problem - The problem structure updated
%    Population- The population structure updated
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 03-06-2009

% No success iteration at begining
Success=false;


if(Problem.Vectorized)
    % Call the penalty function for all active particles in a vectorized way
    % This is just a question of memory storage, as PenaltyEval can
    % manage a vector of points
    
    % Get all active particles in the swarm
    ActiveParticlesIdx=find(Population.Active(:)==1);
    ActiveParticles=Population.x(ActiveParticlesIdx,:);
    [Problem,ObjValue]=...
        PenaltyEval(Problem, ActiveParticles, 0, varargin{:});
    for i=1:length(ActiveParticlesIdx)
        % Was progress attained for the current particle?
        if Population.fy(ActiveParticlesIdx(i))>ObjValue(i)
            % Yes. Update best particle position
            Population.fy(ActiveParticlesIdx(i))=ObjValue(i);
            Population.y(ActiveParticlesIdx(i),:)=Population.x(ActiveParticlesIdx(i),:);
            
            % Check if new leader is available
            if Population.fy(Population.Leader)>Population.fy(ActiveParticlesIdx(i))...
                    || Population.Leader==ActiveParticlesIdx(i)
                Population.Leader=ActiveParticlesIdx(i);
                % Particle swarm iteration declared as successful
                Success=true;
                % Reset last success direction for pattern search
                Problem.Poll.LastSuccess=[];
            end
        end
    end
else
    % For all particles in the swarm
    for i=1:Population.Size
        % If particle is active
        if (Population.Active(i))
            % Compute Objective function value
            [Problem,ObjValue]=...
                PenaltyEval(Problem, Population.x(i,:), 0, varargin{:});
            % Was progress attained for the current particle?
            if Population.fy(i)>ObjValue
                % Yes. Update best particle position
                Population.fy(i)=ObjValue;
                Population.y(i,:)=Population.x(i,:);
                
                % Check if new leader is available
                if Population.fy(Population.Leader)>Population.fy(i) || Population.Leader==i
                    Population.Leader=i;
                    % Particle swarm iteration declared as successful
                    Success=true;
                    % Reset last success direction for pattern search
                    Problem.Poll.LastSuccess=[];
                end
            end
        end
    end
end

end




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file PenaltyEval.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Problem,ObjValue] = PenaltyEval(Problem, x, step, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subrotine PenaltyEval
%  This subrotine is called to evaluate the objective function
%
%  Input:
%    Problem - The problem structure
%    x - Real part of point to evaluate
%    varargin - Extra parameters for objective function
%
%  Output:
%    Problem - The problem structure updated
%    ObjValue - Objective function value. Returns +Inf for unfeasible
%       points
%
% Copyright (C) 2007 A. Ismael F. Vaz and L. N. Vicente.
%
% http://www.norg.uminho.pt/aivaz
% http://www.mat.uc.pt/~lnv
%
% This library is free software; you can redistribute it and/or
% modify it under the terms of the GNU Lesser General Public
% License as published by the Free Software Foundation; either
% version 2.1 of the License, or (at your option) any later version.
%
% This library is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with this library; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%aivaz@dps.uminho.pt 06/12/2006

% Bound feasibility is enforced by the projection onto the bound feasible
% domain
% LB <= x' && x'<=UB
% Be paranoic


  % There is no need to check for Problem.Vectorized, since we are assuming a
  % matrix with NPoints for penalty function evaluation
  [NPoints,Vars]=size(x);
  ObjValue(1:NPoints)=+Inf;

  % calls to the barrier/penalty function
  % we account for each point
  Problem.Stats.PenaltyFunCounter=Problem.Stats.PenaltyFunCounter+NPoints;


  % Check for indices of points that are bound feasible
  FeasibleBoundsIdx=find(min(x'-repmat(Problem.LB,1,NPoints))>=0 & min(repmat(Problem.UB,1,NPoints)-x')>=0);

  if Problem.mLinear==0
    FeasibleLinearIdx=FeasibleBoundsIdx;
  else
      FeasibleLinearIdx=find(max(Problem.A*x(FeasibleBoundsIdx,:)'-repmat(Problem.b,1,length(FeasibleBoundsIdx)))<=0);
  end

  PointsEval=x(FeasibleLinearIdx,:)';
  [Vars,NPoints]=size(PointsEval);

  Problem.Stats.ObjFunCounter=Problem.Stats.ObjFunCounter+NPoints;

  % Check for cache hits on points
  if(Problem.Cache && ~isempty(PointsEval))
      for i=NPoints:-1:1
          [yes,fx,Problem.CacheData]=CheckCache(Problem.CacheData,x(FeasibleLinearIdx(i),:)');
          if(yes)
              % its in cache, therefore we return a cached obj value
              ObjValue(FeasibleLinearIdx(i))=fx;
  %            if(ObjValue(FeasibleLinearIdx(i))>10^-5+feval(Problem.ObjFunction, x(FeasibleLinearIdx(i),:)', varargin{:})...
  %                    || ObjValue(FeasibleLinearIdx(i))<-10^-5+feval(Problem.ObjFunction, x(FeasibleLinearIdx(i),:)', varargin{:}))
  %                ObjValue(FeasibleLinearIdx(i))
  %                feval(Problem.ObjFunction, x(FeasibleLinearIdx(i),:)', varargin{:})
  %                error('POOOOOOOO');
  %            end
              PointsEval(:,FeasibleLinearIdx(i))=[];
              FeasibleLinearIdx(i)=[];
              NPoints=NPoints-1;
          end
      end
  end

  % Compute objective function value for remaining points
  if(~isempty(FeasibleLinearIdx))
      try
          %ObjValue(FeasibleLinearIdx)=feval(Problem.ObjFunction, PointsEval, varargin{:});
          ObjValue(FeasibleLinearIdx)=Problem.ObjFunction(PointsEval, varargin{:}); %KE
          % update counter
          Problem.Stats.RealObjFunCounter=Problem.Stats.RealObjFunCounter+NPoints;
          % Insert points in cache
          if(Problem.Cache)
              for i=1:NPoints
                  Problem =...
                      InsertCache(Problem,PointsEval(:,i),ObjValue(FeasibleLinearIdx(i)),step);
              end
          end

      catch
          error('pswarm:ObjectiveError', ...
              ['Cannot continue because user supplied objective function' ...
              ' failed with the following error:\n%s'], lasterr)
      end
  end

end


function [yes,fx,CacheData]=CheckCache(CacheData,x)
  % Check if point x is in cache.

  xnorm=norm(x,inf);

  % exclude points far away in norm
  Idx = find((abs(CacheData.xnorm(1:CacheData.Counter) - xnorm) < CacheData.Tolerance));
  if isempty(Idx)
      yes=0;
      fx=+Inf;
      return;
  end

  % exclude points far away (norm 1 of the distance)
  points = [CacheData.Point(Idx).x];
  Idx = Idx(max(abs(points - repmat(x,1,length(Idx))),[],1) < CacheData.Tolerance);
  yes = ~isempty(Idx);
  % returning first in cache
  if(yes)
      fx= CacheData.Point(Idx(1)).fx;
  else
      fx= +Inf;
  end

  CacheData.hits=CacheData.hits+1;

end


function Problem = InsertCache(Problem,x,fx,step)
  % Insert point in cache

  % Check cache size. If needed alocate a new chunck of memory
  if(Problem.CacheData.Counter>=length(Problem.CacheData.xnorm))
      % allocate a new chunck
      Idx=1+length(Problem.CacheData.xnorm):Problem.CacheChunks+length(Problem.CacheData.xnorm);
      [Problem.CacheData.Point(Idx).x]=deal(zeros(Problem.Variables,1));
      [Problem.CacheData.Point(Idx).fx]=deal(0);
      [Problem.CacheData.xnorm(Idx)]=deal(0);
      [Problem.CacheData.step(Idx)]=deal(0);
  end


  % Insert point
  Problem.CacheData.Counter=Problem.CacheData.Counter+1;

  xnorm=norm(x,inf);
  Problem.CacheData.Point(Problem.CacheData.Counter).x=x;
  Problem.CacheData.Point(Problem.CacheData.Counter).fx=fx;
  Problem.CacheData.xnorm(Problem.CacheData.Counter)=xnorm;
  Problem.CacheData.step(Problem.CacheData.Counter)=step;

end







%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file PollStep.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Problem,Population]=PollStep(Problem, Population, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  subroutine PollStep
%
%  Performe a poll step for the pattern search on the leader particle of
%  the particle swarm.
%
%  Input:
%    Problem - Problem structure
%    Population - The population
%
%  Output:
%    Poblem - Problem structure updated
%    Population - Population updated
%
% Copyright (C) 2007 A. Ismael F. Vaz and L. N. Vicente.
%
% http://www.norg.uminho.pt/aivaz
% http://www.mat.uc.pt/~lnv
%
% This library is free software; you can redistribute it and/or
% modify it under the terms of the GNU Lesser General Public
% License as published by the Free Software Foundation; either
% version 2.1 of the License, or (at your option) any later version.
%
% This library is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with this library; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 30/03/2007

%
% Proceed with a poll step on the leader
%

  % Columnwise search directions. Computes active linear
  % contraints and returns the search directions (basis for tangent cone)
  switch Problem.TangentCone
      case 0
          [Problem,D]=GetSearchDirectionsSIDPSM(Problem,Population,varargin{:});
      case 1
          [Problem,D]=GetSearchDirectionsNOMAD(Problem,Population,varargin{:});
      case 2
          [Problem,D]=GetSearchDirections(Problem,Population,varargin{:});
      case 3
          [Problem,D]=GetSearchDirectionsNullSpace(Problem,Population,varargin{:});
      case 4
          [Problem,D]=GetSearchDirectionsQR(Problem,Population,varargin{:});
      otherwise
          error('Unknown Tangent Cone parameter');
  end

  % remove equal directions or null directions
  D=CleanDirections(Problem,D);

  % Columnwise Leader
  Leader=Population.y(Population.Leader,:)';

  % sort directions
  if(Problem.PollSort==1)
      D=SortDirections(Problem,Leader,D);
  end


  % Warn user if leader point is unfeasible. Convergence is not guaranteed
  % in this case and it shouldn't happen
  if  ~Problem.FeasWarning && Problem.mLinear>0 && max(Problem.A*Leader-Problem.b)>eps    
      error('Leader particle is unfeasible.');
      fprintf('\n\nPattern search can only get guaranteed convergence if Leader is feasible.\n');
      fprintf('\nSomething bad is going to happen!!\n\n\n');
      Problem.FeasWarning=1;
  end

  %plot(Leader(1),Leader(2),'*b');

  if Problem.Vectorized
      % Since we are evaluating in a vectorized way, the algorithm is
      % non-oportunistic
      Trials=Projection(repmat(Leader,1,size(D,2))+Population.Delta*D,Problem.LB,Problem.UB);
      [Problem,ObjTrials]= PenaltyEval(Problem, Trials', 1, varargin{:});
      [Best,BestIdx]= min(ObjTrials);
      if Population.fy(Population.Leader)>Best
          % a successful poll step. Update counter.
          Problem.Stats.SuccPollSteps=Problem.Stats.SuccPollSteps+1;
          % update leader
          Population.y(Population.Leader,:)=Trials(:,BestIdx);
          Population.fy(Population.Leader)=ObjTrials(BestIdx);

          % Success obtained along the previous successful direction?
          if ~isempty(Problem.Poll.LastSuccess) && ...
                  isequal(Problem.Poll.LastSuccess(:),D(:,BestIdx))
              % Yes. Increase Delta
              %if Population.Delta<Problem.InitialDelta
              Population.Delta=Population.Delta*Problem.IncreaseDelta;
              %end
          else
              % No. Update previous direction
              Problem.Poll.LastSuccess=D(:,BestIdx);
          end

          % Return. Successful poll step
          return;
      end
  else
      % Oportunistic version
      % for all directions
      for i=1:size(D,2)
          % Compute trial point
          Trial=Projection(Leader+Population.Delta*D(:,i),Problem.LB,Problem.UB);
          % Compute objective function
          [Problem,ObjTrial]= PenaltyEval(Problem, Trial', 1, varargin{:});

          % Check for progress
          if Population.fy(Population.Leader)>ObjTrial

              %plot(Trial(1),Trial(2),'*r');

              % a successful poll step. Update counter.
              Problem.Stats.SuccPollSteps=Problem.Stats.SuccPollSteps+1;
              % update leader
              Population.y(Population.Leader,:)=Trial;
              Population.fy(Population.Leader)=ObjTrial;

              % Success obtained along the previous successful direction?
              if ~isempty(Problem.Poll.LastSuccess) && ...
                      isequal(Problem.Poll.LastSuccess(:),D(:,i))
                  % Yes. Increase Delta
                  %if Population.Delta<Problem.InitialDelta
                  Population.Delta=Population.Delta*Problem.IncreaseDelta;
                  %end
              else
                  % No. Update previous direction
                  Problem.Poll.LastSuccess=D(:,i);
              end

              % Return. Successful poll step
              return;
          end
      end
  end

  % No success in poll step. Decrease Delta.
  if Population.Delta>=Problem.Tolerance
      Population.Delta=Population.Delta*Problem.DecreaseDelta;
  end

  Problem.Poll.LastSuccess=[];

end


function [Problem,D]=GetSearchDirectionsSIDPSM(Problem, Population, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% compute all search directions. Coordinate and linear active
% directions - Adapted from SID-PSM
%
% Input:
%   Problem - Problem structure
%   Population - Population structure
%
% Output:
%   D - Search directions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % The best population position. Columnwise
  LeaderPoint=Population.y(Population.Leader,:)';

  % Compute search direction with respect to near active linear constraints
  if Problem.mLinear>0
      Active=Problem.A(find(Problem.A*LeaderPoint-Problem.b >= ...
          -min(Problem.EpsilonActive,10*Population.Delta)),:)';
      NActive=size(Active,2);

      if NActive>0 && NActive<=Problem.Variables
          % Code from Ana and LuÃ­s
          [U,S,V] = svd(Active);
          S1      = S(1:NActive,1:NActive);
          if min(diag(S1)) < Problem.DegTolerance
              Problem.Poll.SearchDirections.Linear = [];
              Problem.Stats.Degenerate=Problem.Stats.Degenerate+1;
          else
              U1  = U(1:Problem.Variables,1:NActive);
              W   = U1 * diag(1./diag(S1)) * V';
              if NActive < Problem.Variables
                  U2 = U(1:Problem.Variables,NActive+1:Problem.Variables);
                  d  = sum(U2',1)';
                  PP=[ -U2 d -W ];
                  Problem.Poll.SearchDirections.Linear=PP(1:Problem.Variables,:);
                  %PP(Problem.RealVariables+1:Problem.n,:);
                  %
                  % Other alternatives:
                  %
                  %           [ U2 -d -W ];
                  %
              else
                  Problem.Poll.SearchDirections.Linear=-W(1:Problem.Variables,:);
              end
          end
      else
          Problem.Poll.SearchDirections.Linear=[];
      end
  else
      Problem.Poll.SearchDirections.Linear=[];
  end


  % The direction are the concatenation of the coordinate, linear,
  % and user directions
  D = [Problem.Poll.SearchDirections.UserSpecified, ...
      Problem.Poll.SearchDirections.Base, ...
      Problem.Poll.SearchDirections.Linear];

end


function [Problem,D]=GetSearchDirectionsNOMAD(Problem, Population, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% compute all search directions. Coordinate and linear active
% directions - Adapted from NOMAD
%
% Input:
%   Problem - Problem structure
%   Population - Population structure
%
% Output:
%   D - Search directions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % The best population position. Columnwise
  LeaderPoint=Population.y(Population.Leader,:)';

  % Feasible domain in NOMAD syntax, so no change in the NOMAD code is
  % requested
  Omega.A = [eye(Problem.Variables), Problem.A']';
  Omega.u = [Problem.UB; Problem.b];
  Omega.l = [Problem.LB; Inf*ones(Problem.mLinear,1)];

  try
      [B,N] = getTangentCone(LeaderPoint,Omega,Problem.EpsilonActive, ones(Problem.Variables,1));
  catch
      disp('');
      disp('NOMAD version of Tangent Cone requests functions:');
      disp(' getTangentCone, scaleDirections, activeConstraints and removeRedundancy');
      disp('');
      disp('Please visit NOMAD homepage and provide these functions');
      error('NOMAD functions missing');
  end



  % The direction are the concatenation of the coordinate, linear,
  % and user directions
  D = [Problem.Poll.SearchDirections.UserSpecified N -N B -B];

end


function [Problem,D]=GetSearchDirections(Problem, Population, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% compute all search directions. Coordinate and linear active
% directions
%
% Input:
%   Problem - Problem structure
%   Population - Population structure
%
% Output:
%   D - Search directions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % The best population position. Columnwise
  LeaderPoint=Population.y(Population.Leader,:)';

  ActiveThreshold=min(Problem.EpsilonActive,10*Population.Delta); % should be <=1
  ActiveThresholdLimit=min(0.1,ActiveThreshold^2);

  % Compute search direction with respect to near active linear constraints
  if Problem.mLinear>0
      while (ActiveThreshold>=ActiveThresholdLimit) % to be flexible with active constraints

          Active=Problem.A(find(Problem.A*LeaderPoint-Problem.b >= ...
              -ActiveThreshold*Problem.Scale),:)';
          NActive=size(Active,2);

          if NActive>0 && NActive<=Problem.Variables
              % Code from Ana and LuÃ­s
              [U,S,V] = svd(Active);
              S1      = S(1:NActive,1:NActive);
              if min(diag(S1)) < Problem.DegTolerance
                  ActiveThreshold=ActiveThreshold/2;
              else
                  U1  = U(1:Problem.Variables,1:NActive);
                  W   = U1 * diag(1./diag(S1)) * V';
                  if NActive < Problem.Variables
                      U2 = U(1:Problem.Variables,NActive+1:Problem.Variables);
                      d  = sum(U2',1)';
                      PP=[ -U2 d -W ];
                      Problem.Poll.SearchDirections.Linear=PP(1:Problem.Variables,:);
                      %PP(Problem.RealVariables+1:Problem.n,:);
                      %
                      % Other alternatives:
                      %
                      %           [ U2 -d -W ];
                      %
                  else
                      Problem.Poll.SearchDirections.Linear=-W(1:Problem.Variables,:);
                  end
                  break; % break while
              end
          else
              Problem.Poll.SearchDirections.Linear=[]; % to be defined
              if NActive<=0
                  break; % break while, since we have no active constraints
              else
                  ActiveThreshold=ActiveThreshold/2; % Attempt to reduce the number of active constraints
              end
          end
      end
  else
      Problem.Poll.SearchDirections.Linear=[];
  end

  if ActiveThreshold<ActiveThresholdLimit
      Problem.Poll.SearchDirections.Linear = [];
      Problem.Stats.Degenerate=Problem.Stats.Degenerate+1;
  end


  % The direction are the concatenation of the coordinate, linear,
  % and user directions
  D = [Problem.Poll.SearchDirections.UserSpecified,...
      Problem.Poll.SearchDirections.Base, ...
      Problem.Poll.SearchDirections.Linear];

end



function [Problem,D]=GetSearchDirectionsQR(Problem, Population, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% compute all search directions. Coordinate and linear active
% directions
%
% Input:
%   Problem - Problem structure
%   Population - Population structure
%
% Output:
%   D - Search directions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % The best population position. Columnwise
  LeaderPoint=Population.y(Population.Leader,:)';

  ActiveThreshold=min(Problem.EpsilonActive,10*Population.Delta); % should be <=1
  ActiveThresholdLimit=min(0.1,ActiveThreshold^2);



  % Compute search direction with respect to near active linear constraints
  if Problem.mLinear>0
      Problem.Poll.SearchDirections.Linear=[]; % to be defined
      while (ActiveThreshold>=ActiveThresholdLimit) % to be flexible with active constraints

          LActive=Problem.A(find(Problem.A*LeaderPoint-Problem.b >= ...
              -ActiveThreshold*Problem.Scale),:)';
          I=eye(Problem.Variables);
          LBActive=...
              -I(find(LeaderPoint-Problem.LB<=ActiveThreshold*Problem.Scale),:);
          UBActive=...
              I(find(Problem.UB-LeaderPoint<=ActiveThreshold*Problem.Scale),:);

          Active=[LActive, LBActive', UBActive'];

          NActive=size(Active,2);

          if NActive>0 && NActive<=Problem.Variables...
                  && (rank(Active) == min(size(Active)))
              [Q,R] = qr(Active,0);
              B = Q/R';
              N = I - B*Active';
              for i=Problem.Variables:-1:1
                  if N(:,i)'*N(:,i)<Problem.Tolerance
                      N(:,i)=[];
                  end
              end
              for i=NActive:-1:1
                  if B(:,i)'*B(:,i)<Problem.Tolerance
                      B(:,i)=[];
                  end
              end

              Problem.Poll.SearchDirections.Linear = [N, -N, B, -B];
              break;
          else
              if NActive<=0
                  break; % break while, since we have no active constraints
              else
                  ActiveThreshold=ActiveThreshold/2; % Attempt to reduce the number of active constraints
              end
          end
      end
  else
      Problem.Poll.SearchDirections.Linear=[];
  end

  if ActiveThreshold<ActiveThresholdLimit
      Problem.Poll.SearchDirections.Linear = [];
      Problem.Stats.Degenerate=Problem.Stats.Degenerate+1;
  end


  % The direction are the concatenation of the linear,
  % and user directions
  if(~isempty(Problem.Poll.SearchDirections.Linear))
      D = [Problem.Poll.SearchDirections.UserSpecified,...
          Problem.Poll.SearchDirections.Linear];
  else
      D = [Problem.Poll.SearchDirections.UserSpecified,...
          Problem.Poll.SearchDirections.Base];
  end

end




function [Problem,D]=GetSearchDirectionsNullSpace(Problem, Population, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% compute all search directions. Coordinate and linear active
% directions
%
% Directions are NullSpace of active constraints
%
% Input:
%   Problem - Problem structure
%   Population - Population structure
%
% Output:
%   D - Search directions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % The best population position. Columnwise
  LeaderPoint=Population.y(Population.Leader,:)';


  % Compute search direction with respect to near active linear constraints
  if Problem.mLinear>0 % We have linear constraints

      Active=find(Problem.A*LeaderPoint-Problem.b+...
          min(Problem.EpsilonActive,10*Population.Delta)*Problem.Scale>0);

      if ~isempty(Active)
          [Closest, Closesti] = min(max(Problem.A(Active,:)*LeaderPoint-Problem.b(Active)));
      else
          Closesti=0;
      end

      if Closesti>0 % We have an active linear constraint
          %Z = NullSpace(Problem.A(Closesti,:)');
          %Problem.Poll.SearchDirections.Linear=[Z, -Z];
          [Q,R] = qr(Problem.A(Closesti,:)',0);
          B = Q/R';
          N = eye(Problem.Variables) - B*Problem.A(Closesti,:);
          Problem.Poll.SearchDirections.Linear = [Problem.A(Closesti,:)', -Problem.A(Closesti,:)', N, -N, B, -B];
      end
  else
      Problem.Poll.SearchDirections.Linear=[];
  end


  % The direction are the concatenation of the coordinate, linear,
  % and user directions
  D = [Problem.Poll.SearchDirections.UserSpecified,...
      Problem.Poll.SearchDirections.Base, ...
      Problem.Poll.SearchDirections.Linear];

end


function [D]=CleanDirections(Problem,D)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Cleans the search directions set D by removing the null directions and
% the repeated directions
%
% Input:
%   Problem - Problem structure
%   D - Set of search directions
%
% Output:
%   D - Set of search directions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% remove any null directions 
  [n,m]=size(D);
  for i=m:-1:1 % decreasing order, since we may remove some directions
      if norm(D(:,i))<Problem.Tolerance
          D(:,i)=[];
      end
  end

  % remove repeated directions
  [n,m]=size(D); %It should be faster then to update m in the previous cycle
  for i=m:-1:1 %decreasing
      for j=i-1:-1:1 % decreasing
          if norm(D(:,i)-D(:,j))<Problem.Tolerance
              D(:,i)=[]; % remove direction with higher indice
              break; % break internal cycle
          end
      end
  end
end


function [D]=SortDirections(Problem,Leader,D)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Sort the search directions
%
% Input:
%   Problem - Problem structure
%   D - Set of search directions
%
% Output:
%   D - Set of search directions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  switch Problem.SearchType
      case 2
          % In the RBF case we are sorting by the RBF model value

          % get the model data, if available
          if(isappdata(Problem.RBFDataHandle,'RBFData'))
              RBFData = getappdata(Problem.RBFDataHandle,'RBFData');
              if(~isempty(RBFData))
                  nDir=size(D,2);
                  modelf=zeros(1,nDir);
                  for i=1:nDir
                      [modelf(i),grad_f] = RBFObjFun(Leader+D(:,i), RBFData.Y, RBFData.lambda, RBFData.n, RBFData.np);
                  end
                  [modelfs,idx]=sort(modelf);
                  D(1:Problem.Variables,1:nDir)=D(1:Problem.Variables,idx);
              end
          end

      case 3
          % In the MFN case we are sorting by the MFN model value

          % get the model data, if available
          if(isappdata(Problem.MFNDataHandle,'MFNData'))
              MFNData = getappdata(Problem.MFNDataHandle,'MFNData');
              if(~isempty(MFNData))
                  nDir=size(D,2);
                  modelf=zeros(1,nDir);
                  for i=1:nDir
                      [modelf(i),grad_f] = QuadObjFun(Leader+D(:,i), MFNData.H, MFNData.g);
                  end
                  [modelfs,idx]=sort(modelf);
                  D(1:Problem.Variables,1:nDir)=D(1:Problem.Variables,idx);
              end
          end
      otherwise
          display('Poll step with an unknowing sorting strategy');
  end
  
end




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file Projection.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function X=Projection(X, L, U)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  subrotine Projection
%
%  Input:
%    X - Point to be projected
%    L - Domain lower bound
%    U - Domain upper bound
%
%  Output:
%    X - Projected point
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 06/12/2006

  %Do you really need comments?

  % X are columnwise vectors!

  if size(X,2)==1
    X = max(L,min(U,X));
  else
    X=max(X,repmat(L,1,size(X,2)));
    X=min(X,repmat(U,1,size(X,2)));
  end

  % for i=1:length(X)
  %     if X(i)<L(i)
  %         X(i)=L(i);
  %     end
  %     if X(i)>U(i)
  %         X(i)=U(i);
  %     end
  % end

end










%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file Quad_MFN.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Success,Problem,Population]=Quad_MFN(Problem, Population, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The Quadratic Minimum Forbenious Norm model
%
%
%
%  Input:
%    Problem - The problem structure
%    Population- The population structure
%
%  Output:
%    Problem - The problem structure updated
%    Population- The population structure updated
%    Success - if the search step led to success
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 05-09-2009
% Part of the code is imported from the SID-PSM solver

  % No successful step at begining
  Success=0;


  % Reset data from previous model
  %RBFData=[];
  %setappdata(Problem.RBFDataHandle,'RBFData',RBFData);

  % Check for enough available point in the cache
  if(Problem.CacheData.Counter<Problem.Variables+2)
      % more than n+2 points are requested to be in the cache
      return;
  end


  [quad,H,g] = quad_Frob(Population.y(Population.Leader,:)',...
      [Problem.CacheData.Point(max(1,Problem.CacheData.Counter-50*(Problem.Variables+1)):Problem.CacheData.Counter).x],...
      [Problem.CacheData.Point(max(1,Problem.CacheData.Counter-50*(Problem.Variables+1)):Problem.CacheData.Counter).fx],1,1,0);


  if quad~=1
      return;
  end

  % We have enough points to build the model
  Problem.Stats.MFN.nModels=Problem.Stats.MFN.nModels+1;

  if(Problem.PollSort==1)
      % Save data for future use
      MFNData.H=H;
      MFNData.g=g;
      MFNData.n=Problem.Variables;
      MFNData.np=np;
      setappdata(Problem.MFNDataHandle,'MFNData',MFNData);
  end

  switch Problem.TRType
      case 0
          %Thrust region
          if(Problem.Stats.MFN.nModelsPreviousSuccess>0)
              TrustSize=Population.Delta*2;
          else
              TrustSize=Population.Delta;
          end
      case 1
          TrustSize=Population.MFNDelta;
      otherwise
          error('Unknown Trust region strategy for MFN model');
  end

  x = zeros(Problem.Variables,1);

  switch Problem.MFNAlgo
      case 1 % DCA

          f=0;
          gf=zeros(Problem.Variables,1);

          lb=-TrustSize*ones(Problem.Variables,1);
          ub=TrustSize*ones(Problem.Variables,1);

          % DCA2 definitions
          kmax = 3000;
          rhomax = max(eig(H));
          rho = Problem.DCARhoFactor*rhomax;
          % End of DCA2 definitions

          relerror = 1;
          k = 0;

          while (relerror>1d-6 && k<kmax)

              stop=0;
              while(~stop)
                  y = rho*x - (H*x+g);

                  % Projection over the feasible set
                  yaux = y/rho;
                  masku = ( yaux > ub ); yaux(masku) = ub(masku); % Thrust region
                  maskl = ( yaux < lb ); yaux(maskl) = lb(maskl);
                  % End of projection to the feasible set

                  [fyaux,gfyaux] = QuadObjFun(yaux,H,g);
                  if(fyaux<f)
                      stop=1;
                  else
                      if(rho>=rhomax)
                          stop=1;
                          % No progress and rho is too large. Should we abort
                          % or let the algorithm run with a different point
                          yaux=x;
                          fyaux=f;
                          gfyaux=gf;
                      else
                          rho=2*rho;
                      end
                  end
              end

              relerror = norm(yaux-x);

              x = yaux;
              f=fyaux;
              gf=gfyaux;

              k = k+1;

          end

          dir=x;

      case 2

          % Trust region bounds
          lb=-TrustSize*ones(Problem.Variables,1);
          ub=TrustSize*ones(Problem.Variables,1);

          options = optimset('GradObj','on','Display','off','Diagnostics','off','TolFun',1d-6,'TolX',1d-6);
          dir=fmincon('QuadObjFun',x,[],[],[],[],lb,ub,[],options, H, g);

      case 3

          error('In order to use DIRECT as the quadratic model minimizer you need to obtain the DIRECT solver and to comment this error message');

          % Trust region bounds
          lb=-TrustSize*ones(Problem.Variables,1);
          ub=TrustSize*ones(Problem.Variables,1);

          DirectProblem.f='QuadObjFun';
          DirectBounds=zeros(2,length(lb))';
          DirectBounds(:,1)=lb;
          DirectBounds(:,2)=ub;
          DirectOpts.maxevals=2000;      % maximum number of function evaluations
          DirectOpts.maxits=2000;       % disable maximum number of iterations
          DirectOpts.maxdeep=100;        % maximum number of rectangle divisions
          DirectOpts.showits=0;

          [f,dir]=Direct(DirectProblem,DirectBounds,DirectOpts, H, g);

      case 4

          error('In order to use trust as the quadratic model minimizer you need to obtain the trust solver available in the optimization toolbox and to comment this error message');

          dir=trust(g,H,TrustSize); %Population.Delta * 2 * sqrt(2)

      otherwise
          error('Unknown algorithm for MFN minimization');
  end

  x=Population.y(Population.Leader,:)'+dir;

  %plotMFN(Problem.Stats.MFN.nModels, lb, ub, H,g,Problem.Variables,np,x);

  % Damp point if linear constraints are present
  if Problem.mLinear>0
      x=DampingLinearConstraints(Problem,Population,x);
  end


  % Evaluate true objective function
  [Problem,ObjValue] = PenaltyEval(Problem, x', 0, varargin{:});

  % Did we have any progress in the model?
  if(Population.fy(Population.Leader)>ObjValue)
      % Success
      Success=1;
      Population.y(Population.Leader,:)=x';
      %        display('Progress');
      %        Population.fy(Population.Leader)-ObjValue
      Population.fy(Population.Leader)=ObjValue;

      % We have success in the model
      Problem.Stats.MFN.Success=Problem.Stats.MFN.Success+1;
  end

end



function [x]=DampingLinearConstraints(Problem,Population,x) %% 1

  % x is bound feasible

  dx=x-Population.y(1,:)';

  AlphaMaxLinCons=1.0;

  I=find(Problem.A*dx>0);
  if ~isempty(I)
      % We are not allowing a step longer than 1
      AlphaMaxLinCons=...
          min(AlphaMaxLinCons,min((Problem.b(I)-Problem.A(I,:)*Population.y(1,:)')...
          ./(Problem.A(I,:)*dx)));
  end

  if(AlphaMaxLinCons>0)
      x=Projection(Population.y(1,:)'+(AlphaMaxLinCons.*dx),...
          Problem.LB(1:Problem.Variables), ...
          Problem.UB(1:Problem.Variables));
  else
      x=Population.y(1,:)';
  end
end









%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file QuadObjFun.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [f,grad_f] = QuadObjFun(x, H, g)

  % It computes the function value and gradient 
  % ---> for use in fmincom and DCA2

  f = 0.5*x'*H*x+g'*x;
  grad_f = H*x+g;
end










%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file RBF.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Success,Problem,Population]=RBF(Problem, Population, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The RFB function
%
%
%
%  Input:
%    Problem - The problem structure
%    Population- The population structure
%
%  Output:
%    Problem - The problem structure updated
%    Population- The population structure updated
%    Success - if the search step led to success
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 16-06-2009
% lnv@mat.uc.pt       16-06-2009

  % No successful step at begining
  Success=0;


  % Reset data from previous model
  %RBFData=[];
  %setappdata(Problem.RBFDataHandle,'RBFData',RBFData);

  % Check for enough available point in the cache
  if((Problem.CacheData.Counter<Problem.Variables+2 && ~Problem.RBFPoints) ||...
          (sum(Problem.CacheData.step)<Problem.Variables+2 && Problem.RBFPoints))
      return;
  end

  % We have enough points to build the model
  Problem.Stats.RBF.nModels=Problem.Stats.RBF.nModels+1;

  % Get Y and fY values for model build
  nmax=(Problem.Variables+1)*(Problem.Variables+2)/2;

  if(Problem.CacheData.Counter<=nmax)
      np=Problem.CacheData.Counter;
      Y = [Problem.CacheData.Point(1:np).x];
      fY = [Problem.CacheData.Point(1:np).fx];
  else
      np=nmax;
      Far=ceil(0.2*nmax); % 20 percent far far away :)
      Near=nmax-Far;      % remaining 80 percent nearby
      start=max(1,Problem.CacheData.Counter-50*(Problem.Variables+1));
      Y = [Problem.CacheData.Point(start:start+Far-1).x...
          Problem.CacheData.Point(Problem.CacheData.Counter-Near+1:Problem.CacheData.Counter).x];
      fY = [Problem.CacheData.Point(start:start+Far-1).fx...
          Problem.CacheData.Point(Problem.CacheData.Counter-Near+1:Problem.CacheData.Counter).fx];
  end

  %meanfY=mean(fY);
  %fY(fY>meanfY)=meanfY;

  % Build A
  % Preallocating memory
  A=zeros(np,np);
  % A is symmetric
  for i=1:np
      for j=i+1:np
          A(i,j) = norm(Y(:,i)-Y(:,j))^3;
      end
      A(i+1:np,i)=A(i,i+1:np);
  end


  A = [ A [ ones(np,1) Y'] ;
      [ ones(np,1) Y']' zeros(Problem.Variables+1,Problem.Variables+1) ];
  rhs = [ fY' ; zeros(Problem.Variables+1,1) ];

  % solve the linear system
  if(condest(A)>eps && condest(A)<1/eps)
      lambda = A\rhs;
  else
      [U,S,V]=svd(A);
      s = diag(S);
      e = zeros(length(s),1);
      ind = s/max(abs(s)) >= eps;
      e(ind) = 1./s(ind);
      lambda = V*diag(e)*U'*rhs;
  end

  if(~isfinite(lambda))
      return;
  end

  if(Problem.PollSort==1)
      % Save data for future use
      RBFData.Y=Y;
      RBFData.lambda=lambda;
      RBFData.n=Problem.Variables;
      RBFData.np=np;
      setappdata(Problem.RBFDataHandle,'RBFData',RBFData);
  end

  switch Problem.TRType
      case 0
          %Thrust region
          if(Problem.Stats.RBF.nModelsPreviousSuccess>0)
              TrustSize=Population.Delta*2;
          else
              TrustSize=Population.Delta;
          end
      case 1
          TrustSize=Population.RBFDelta;
      otherwise
          error('Unknown Trust region strategy for RBF model');        
  end


  % Initial guess for DCA
  %x = zeros(Problem.Variables,1);
  x = Population.y(Population.Leader,:)';

  % Trust region bounds
  lb=max(Problem.LB,x-TrustSize*ones(Problem.Variables,1));
  ub=min(Problem.UB,x+TrustSize*ones(Problem.Variables,1));

  [f,gf] = RBFObjFun(x,Y,lambda,Problem.Variables,np);
  finitial=f;

  if(norm(gf)<1d-6)
      % Initial guess is stationary point
      % Do a random preturbation
      x = x + (ub-lb).*(rand(Problem.Variables,1)-0.5);
      [f,gf] = RBFObjFun(x,Y,lambda,Problem.Variables,np);
      %    display('Randomly perturbing initial guess');
  end


  switch Problem.RBFAlgo
      case 1 % DCA

          % DCA2 definitions
          kmax = 3000;
          rhomax = max(abs(lambda(1:np)))*6*2*max(norm(lb),norm(ub))*np;
          rho = Problem.DCARhoFactor*rhomax;
          % End of DCA2 definitions

          relerror = 1;
          k = 0;

          while (relerror>1d-6 && k<kmax)

              stop=0;
              while(~stop)
                  y = rho*x - (gf - lambda(np+2:np+Problem.Variables+1));

                  % Projection over the feasible set
                  yaux = (y - lambda(np+2:np+Problem.Variables+1))/rho;
                  masku = ( yaux > ub ); yaux(masku) = ub(masku); % Thrust region
                  maskl = ( yaux < lb ); yaux(maskl) = lb(maskl);
                  % End of projection to the feasible set

                  [fyaux,gfyaux] = RBFObjFun(yaux,Y,lambda,Problem.Variables,np);
                  if(fyaux<f)
                      stop=1;
                  else
                      if(rho>=rhomax)
                          stop=1;
                          % No progress and rho is too large. Should we abort
                          % or let the algorithm run with a different point
                          yaux=x;
                          fyaux=f;
                          gfyaux=gf;
                      else
                          rho=2*rho;
                      end
                  end
              end

              relerror = norm(yaux-x);

              x = yaux;
              f=fyaux;
              gf=gfyaux;

              k = k+1;

          end

      case 2

          options = optimset('GradObj','on','Display','off','Diagnostics','off','TolFun',1d-5,'TolX',1d-5);
          [x,f]=fmincon('RBFObjFun',x,[],[],[],[],lb,ub,[],options, Y, lambda, Problem.Variables, np);

      case 3

          error('In order to use DIRECT as the RBF model minimizer you need to obtain the DIRECT solver and to comment this error message');

          DirectProblem.f='RBFObjFun';
          DirectBounds=zeros(2,length(lb))';
          DirectBounds(:,1)=lb;
          DirectBounds(:,2)=ub;
          DirectOpts.maxevals=2000;      % maximum number of function evaluations
          DirectOpts.maxits=2000;       % disable maximum number of iterations
          DirectOpts.maxdeep=100;        % maximum number of rectangle divisions
          DirectOpts.showits=0;

          [f,x]=Direct(DirectProblem,DirectBounds,DirectOpts, Y, lambda, Problem.Variables, np);

      otherwise
          error('Unknown algorithm for RBF minimization');
  end

  %plotRBF(Problem.Stats.RBF.nModels, lb, ub, Y,lambda,Problem.Variables,np,x);

  % Damp point if linear constraints are present
  if Problem.mLinear>0
      x=DampingLinearConstraints(Problem,Population,x);
  end


  % Evaluate true objective function
  [Problem,ObjValue] = PenaltyEval(Problem, x', 0, varargin{:});

  % Did we have any progress in the model?
  switch Problem.TRType
      case 0
          if(Population.fy(Population.Leader)>ObjValue)
              % Success
              Success=1;
              Population.y(Population.Leader,:)=x';
              %        display('Progress');
              %        Population.fy(Population.Leader)-ObjValue
              Population.fy(Population.Leader)=ObjValue;

              % We have success in the model
              Problem.Stats.RBF.Success=Problem.Stats.RBF.Success+1;
          end
      case 1
          if(finitial-f>0)

              TRRho=(Population.fy(Population.Leader)-ObjValue)/(finitial-f);

              if(TRRho>=Problem.RBFEta1)
                  Population.RBFDelta=min(Problem.RBFGamma1*Population.RBFDelta,Problem.RBFDeltaMax);
              elseif (TRRho<Problem.RBFEta0)
                  Population.RBFDelta=Problem.RBFGamma0*Population.RBFDelta;
              end

              if(TRRho>Problem.RBFEta0)
                  % Success
                  Success=1;
                  Population.y(Population.Leader,:)=x';
                  %        display('Progress');
                  %        Population.fy(Population.Leader)-ObjValue
                  Population.fy(Population.Leader)=ObjValue;

                  % We have success in the model
                  Problem.Stats.RBF.Success=Problem.Stats.RBF.Success+1;
              end
          end
  end

end







%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file RBFObjFun.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [f,grad_f] = RBFObjFun(x, Y, lambda, n, np)

  % It computes the function value and gradient 
  % ---> for use in fmincom and DCA2

  f = 0;
  grad_f = zeros(n,1);
  for i=1:np
      v = x-Y(:,i);
      nv = norm(v);
      f = f + lambda(i)*nv^3;
      grad_f = grad_f + lambda(i)*3*nv*v;
  end

  f = f + lambda(np+1) + lambda(np+2:np+n+1)'*x;
  grad_f = grad_f + lambda(np+2:np+n+1);
end









%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copied from file UpdateInfo.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Problem,Population]=UpdateInfo(Problem, Population)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subrotine UpdateInfo
%
%  This subroutine is called after the search and poll step
%
%
%  Input:
%    Problem - The problem structure
%    Population- The population structure
%
%  Output:
%    Problem - The problem structure updated
%    Population- The population structure updated
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% aivaz@dps.uminho.pt 03-06-2009

  switch Problem.SearchType
      case 0 % empty search step
          % nothing to do
      case 1 % Particle swarm
          [Problem,Population]=UpdateSwarmInfo(Problem, Population);
      case {2,3} % RBF or MFN

      otherwise
          error('Unknown search step type')
  end
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subrotine UpdateInfo
%
%  This subroutine is called after the search and poll step
%  Particle swarm particles update
%
%
%  Input:
%    Problem - The problem structure
%    Population- The population structure
%
%  Output:
%    Problem - The problem structure updated
%    Population- The population structure updated
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Problem,Population]=UpdateSwarmInfo(Problem, Population)
  % Compute inercia.
  Inercia = Problem.InerciaInitial - ...
      (Problem.InerciaInitial-Problem.InerciaFinal)*...
      Problem.Stats.IterCounter/Problem.MaxIterations;

  % Update velocity and new particle positions
  % For all particles
  for i=1:Population.Size
      % Is active?
      if Population.Active(i)
          % Update velocity for real variables
          Population.vx(i,:)=Projection((Inercia*Population.vx(i,:)+ ...
              Problem.Cognitial*unifrnd(0,ones(1,Problem.Variables)).*...
              (Population.y(i,:)-Population.x(i,:))+...
              Problem.Social*unifrnd(0,ones(1,Problem.Variables)).*...
              (Population.y(Population.Leader,:)-Population.x(i,:)))',...
              -Problem.MaxVelocityVect,Problem.MaxVelocityVect);
          % Update particle position and check bound limits


          AlphaMax=ones(Problem.Variables,1);
          % check for simple bound
          I=find(Population.vx(i,:)<0); % we could violate lower bounds
          if ~isempty(I)
              AlphaMax(I)=...
                  min(AlphaMax(I),(Problem.LB(I)-Population.x(i,I)')...
                  ./Population.vx(i,I)');
          end
          I=find(Population.vx(i,:)>0); % we could violate upper bounds
          if ~isempty(I)
              AlphaMax(I)=...
                  min(AlphaMax(I),(Problem.UB(I)-Population.x(i,I)')...
                  ./Population.vx(i,I)');
          end

          AlphaMax=max(zeros(Problem.Variables,1),AlphaMax);

          velocity=AlphaMax.*Population.vx(i,:)';

          AlphaMaxLinCons=1.0;
          if Problem.mLinear>0 && Problem.LinearStepSize
              I=find(Problem.A*velocity>0);
              if ~isempty(I)
                  % We are not allowing a step longer than 1
                  AlphaMaxLinCons=...
                      min(AlphaMaxLinCons,min((Problem.b(I)-Problem.A(I,:)*Population.x(i,:)')...
                      ./(Problem.A(I,:)*velocity)));
              end
          end

          if(AlphaMaxLinCons>0)

              Population.x(i,:)=Projection(Population.x(i,:)'+(AlphaMaxLinCons.*velocity),...
                  Problem.LB(1:Problem.Variables), ...
                  Problem.UB(1:Problem.Variables));

              if Problem.mLinear>0 && Problem.LinearStepSize>1 % truncate velocity
                  Population.vx(i,:)=(AlphaMaxLinCons.*velocity)';
              end
          end
      end
  end

  % To compute population norm. Start with Leader.
  MaxVelocity=norm(Population.x(Population.Leader));

  % Reset number of active particles.
  Population.ActiveParticles=0;
  for i=1:Population.Size
      % Check if particle is active and we do not want to remove the
      % leader.
      if Population.Active(i) && Population.Leader~=i
          % compute particle velocity norm (for the stopping criteria.
          VelocityNorm=norm(Population.vx(i,:));
          Distance=norm(Population.x(i,:)-Population.x(Population.Leader,:));
          if Distance<Population.Delta && VelocityNorm<Population.Delta
              % Is neighbour
              Population.Active(i)=false;
          else
              MaxVelocity=MaxVelocity+VelocityNorm;
          end
      end
      % Account for active particles.
      if Population.Active(i)
          Population.ActiveParticles=Population.ActiveParticles+1;
      end
  end

 end